#pragma once
#include <thread>
#include <pthread.h>
#include <sched.h>
#include <errno.h>
#include <string.h>
#include <stdexcept>
#include <vector>

namespace industrial_robot_control {
namespace utils {

class RealtimeUtils {
public:
    /**
     * @brief 设置线程实时优先级
     * @param thread 目标线程
     * @param priority 优先级 (1-99，数值越大优先级越高)
     * @throws std::invalid_argument 当优先级超出范围时
     */
    static void setRealtimePriority(std::thread& thread, int priority) {
        if (priority < 1 || priority > 99) {
            throw std::invalid_argument("实时优先级必须在1-99范围内");
        }

        pthread_t pthread = thread.native_handle();
        struct sched_param param;
        param.sched_priority = priority;
        
        // 设置SCHED_FIFO调度策略
        if (pthread_setschedparam(pthread, SCHED_FIFO, &param) != 0) {
            throw std::runtime_error("设置实时优先级失败: " + std::string(strerror(errno)));
        }
    }

    /**
     * @brief 设置线程CPU亲和性
     * @param thread 目标线程
     * @param core_ids 要绑定的CPU核心ID列表
     * @throws std::runtime_error 当设置失败时
     */
    static void setCpuAffinity(std::thread& thread, const std::vector<int>& core_ids) {
        cpu_set_t cpuset;
        CPU_ZERO(&cpuset);
        
        for (int core : core_ids) {
            CPU_SET(core, &cpuset);
        }

        pthread_t pthread = thread.native_handle();
        if (pthread_setaffinity_np(pthread, sizeof(cpu_set_t), &cpuset) != 0) {
            throw std::runtime_error("设置CPU亲和性失败: " + std::string(strerror(errno)));
        }
    }

    /**
     * @brief 检查当前系统是否支持实时调度
     */
    static bool isRealtimeSupported() {
        // 检查是否有实时调度策略可用
        struct sched_param param;
        param.sched_priority = 1;
        pthread_t pthread = pthread_self();
        
        // 尝试设置实时优先级，然后恢复原来的设置
        int old_policy;
        struct sched_param old_param;
        
        if (pthread_getschedparam(pthread, &old_policy, &old_param) != 0) {
            return false;
        }
        
        int result = pthread_setschedparam(pthread, SCHED_FIFO, &param);
        if (result == 0) {
            // 恢复原来的设置
            pthread_setschedparam(pthread, old_policy, &old_param);
            return true;
        }
        
        return false;
    }
};

} // namespace utils
} // namespace industrial_robot_control
